# coding: utf-8

"""
PCD File reader utility for version 0.7 .
"""

from numpy import array as point_cloud


def load_point_cloud(file_name: str) -> point_cloud:
    """ load points from PCD file.
    :param file_name: the name of the file to read.
    :return : the point array.
    """
    points = []
    with open(file_name, 'r') as f:
        lines = iter(line[:-1] for line in f.readlines())
        if file_name.endswith('pcd'):
            # deal with pcd header.
            line = next(lines)
            while not line.startswith('DATA'):
                line = next(lines)
        for line in lines:
            coord = [float(i) for i in line.split()]
            if len(coord) == 3:
                points.append(coord)
    return point_cloud(points)


def save_point_cloud(file_name: str, points: point_cloud):
    """ Save the point array into file by given the file name.
    :param file_name: the name of file to write.
    :param points: the point array.
    """
    header = '''# .PCD v0.7 - Point Cloud Data file format
VERSION 0.7
FIELDS x y z
SIZE 4 4 4
TYPE F F F
COUNT 1 1 1
WIDTH {0}
HEIGHT 1
VIEWPOINT 0 0 0 1 0 0 0
POINTS {0}
DATA ascii'''
    with open(file_name, 'w') as f:
        print(header.format(len(points)), file=f)
        for p in points:
            print('{0} {1} {2}'.format(*p), file=f)
